#ifndef __JUMP_POINT_SEARCH_H__
#define __JUMP_POINT_SEARCH_H__

#include "path_generate/path_base.h"
#include "path_generate/p2p/grid_node.h"

#include <queue>
#include <unordered_map>
#include <vector>
#include <algorithm>  // for std::reverse
using namespace std;

class JPSPlanner : public PlannerBase {
    public:
        JPSPlanner();
        ~JPSPlanner() = default;

        // 生成路径
        void genGridPath(int sx, int sy, int gx, int gy, vector<pair<int, int>>& points);

        // 裁剪邻居节点
        vector<pair<int, int>> pruneNeighbor(GridNodePtr curr_node);
        // 检查是否为强迫节点
        bool hasForcedNeighbor(GridNodePtr curr_node, int dir_x, int dy);
        // 跳点搜索
        GridNodePtr jump(GridNodePtr curr_node, int dir_x, int dy, GridNodePtr goal);

        // 路径裁剪
        void PathPruning(vector<pair<int, int>>& points_in, vector<pair<int, int>>& out_points);

    private:
        ros::NodeHandle nh_;
        ros::Publisher debugMapPub_;
        ros::Publisher jumpPointPub_;
        set<int> expend_points_;    //扩展点
        set<int> jump_points_;

        nav_msgs::OccupancyGrid debugGrid_;
        RvizMarker rvizMarker_;
};

#endif